
from os import popen
import time
import serial  # 导入串口通信模块
import re  # 提取文本中的特定类型字符
from binascii import a2b_hex
from geometry_msgs.msg import Twist
from threading import Thread
import rospy
import sys
sys.path.append("../")
from log.pylog import log
LOGS_INFO = log.info

portx = "/dev/single_laser"  # COM2口用来读数
bps = 9600
# 设置并打开串口
ser = serial.Serial(portx, int(bps), timeout=1, parity=serial.PARITY_NONE, stopbits=1)  
# 串口执行到这已经打开 再用open命令会报错

if ser.is_open:  # 判断串口是否打开
    print("open success")

'''
description:  16进制数据 截取转换 实际距离
param {*} line 截取得到的参数列表
return {*}
'''
def hex2dig(line):
    digital = 0.0
    flag = 1
    for i,str in  enumerate(line):
        if str== 'e':
            flag = 0
        if str>='0' and str <='9':
            if flag:
                digital += pow(10,2-i) * int(str)
            else:
                digital += pow(10,3-i) * int(str)
    # digital *= 100

    # 距离<4.5，会超过测量范围，导致显示距离为522000
    if digital > 1.0:
        return -1
    return digital


'''
description: 获取激光测距模块测量量，并自动转换数据格式
param {*} ser 串口
return {*}
'''
def get_laser_dis():
    global ser
    while not ser.is_open:
        ser.open()
        print("open serial!")
    dis = 0
    match_score = 0
    complete_flag = True
    while complete_flag:
        bit_list =[]
        bit = ser.read().hex() # 递归读取串口
        # 50 06 83 -> head
        if bit== "50" or bit =="06" or bit=="83":
            match_score += 1
        if match_score == 3:
            for i in range(7):
                bit = ser.read().hex() # 递归读取串口
                bit_list.append(bit[1])
            # print('bit_list: ',bit_list)
            match_score = 0
            dis = hex2dig(bit_list)
            if dis != -1:
                complete_flag = False
    return dis
 

def forwrd_p(cmd_vel,move_cmd):
    rate = 50
    r = rospy.Rate(rate)
    while True:
        if move_cmd.linear.x >= -0.01:
            break
        cmd_vel.publish(move_cmd)
        r.sleep()

def adopt_forward(speed=-0.30,target_dis=0.2):
    global ser
    cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
    move_cmd = Twist()
    # 初始设置速度
    # 设置目标距离
    move_cmd.linear.x = speed
    # 获取真实距离
    laser_dis = get_laser_dis()
    print(laser_dis)
    init_minus_val = laser_dis - target_dis
    forward_thread = Thread(target=forwrd_p,args=[cmd_vel,move_cmd])
    forward_thread.start()
    while laser_dis > target_dis :
        rospy.loginfo('distance of the obstacle : %f', laser_dis)
        laser_dis = get_laser_dis()
        # 速度循环调节
        if speed < -0.05:
            minus_val = laser_dis - target_dis
            speed *= minus_val/init_minus_val 
        if speed < -0.5:
            speed = -0.5  
        move_cmd.linear.x = speed 
        rospy.loginfo("speed:{} ".format(move_cmd.linear.x))
        if speed > -0.02:
            break
    move_cmd.linear.x = 0
    cmd_vel.publish(Twist())
    if forward_thread.isAlive():
        forward_thread.join()
    ser.close()

def send_commond():
    global ser
    # 上电就测
    auto_detect_on = 'FA 04 0D 01 F4'
    auto_detect_off = 'FA 04 0D 00 F5'
    turn_off = '50 06 05 00 A3'
    turn_on = '50 06 05 01 A4'
    '''
    50 06 07 5D
    控制激光 50 06 05 01 
    0000 0110
    0000 0101

    0000 1011
    0101 0000
    0101 1011

    0101 1100
    5C
    1010 0100
    A4
    0101 1011
    5B
    1010 0101
    A3
    '''
    send_word = bytes.fromhex(turn_off)
    ser.write(turn_off)

if __name__ == "__main__":

    send_commond()
    # for i in  range(5):
    #     print(get_laser_dis())
    # ser.close()  # 关闭端串口
    # rospy.init_node('single_scan')
    # # adopt_forward(-0.3,0.1)
    # st_time = time.time()
    # # for i in range(10): 
    # while(1):
    #     print(get_laser_dis())
    # ed_time = time.time()
    # print("cost time{}".format(ed_time - st_time))


    